/*! \file swing_up_motor2.cpp

\brief Swing up controller for Cartpole pendulum system
Date of creation: 2010-15-11

Copyright
Julian David Colorado, jd.colorado@upm.es
Universidad Politecnica de Madrid

See the file "license.terms" for information on usage and redistribution of this file, and for a
DISCLAIMER OF ALL WARRANTIES.

*/

/*
INPUT
\param MAT SimParam: Simulation Parameters 4 x 6
\param VEC X: State vector 4
\param int i: Time flag
\param int ui: Real time

OUTPUT
\param double TAU: Torque input
\param VEC X: State derivate

*/
#include "../../dynlib.h"

void swing_up(Matrix &SimParam,Matrix &X,int i,int ui,double *Tau,ColumnVector &dX)
{
	double signo, temp,Eo,M,I,w,E;
	
	/*swing_up(M][m][L][g][R][r][Km][Kg][teta][dteta][i):
	M=SimParam->me[1][1]
	m=SimParam->me[1][2]
	L=SimParam->me[1][3]
	g=SimParam->me[1][4]
	R=SimParam->me[2][1]
	r=SimParam->me[2][2]
	Km=SimParam->me[2][3]
	Kg=SimParam->me[2][4]*/

	
	//Sign of control action
	if(X(4,i)*cos(X(2,i))>0.0)
		signo=1.0;
	else
		signo=-1.0;
	
	temp=2;
	Eo=0;
	M=(SimParam(1,1)+SimParam(1,2));
	
	//I=(0.0833)*m*L**2
	I=(0.0833)*SimParam(1,2)*(SimParam(1,3)*SimParam(1,3))/2;
	
	//w=sqrt(((M+m)*g*L)/(4*I))
	w=sqrt((M*SimParam(3,5)*SimParam(1,3))/(4*I));
	
	//E=(M+m)*g*L*(0.5*(dteta[1][i+1]/w)**2+cos(teta[1][i+1])-1)
	E=(M)*SimParam(1,4)*SimParam(1,3)*(0.5*((X(4,i)/w)*(X(4,i)/w))+cos(X(2,i))-1);
	
	//u=1/7*((R*r*(M+m))/(Km*Kg))*(temp*(E-Eo)*signo+((Km**2*Kg**2*dteta[0][i+1])/(R*r**2)))
	*Tau=(((SimParam(2,1)*SimParam(2,2)*(M))/(SimParam(2,3)*SimParam(2,4)))*(temp*(E-Eo)*signo+((SimParam(2,3)*SimParam(2,3)*SimParam(2,4)*SimParam(2,4)*X(3,i))/(SimParam(2,1)*SimParam(2,2)*SimParam(2,2)))))*1/7;

}
